function [center,semiAxes,angle] = fitellipse(points) %#codegen
%FITELLIPSE 拟合椭圆 
%
% Input Arguments
% # points: m*2矩阵,m为用于拟合椭圆的点数,m>=6,否则无效
% 
% Output Arguments
% # center: 2*1矩阵,椭圆圆心
% # semiAxes: 2*1矩阵,椭圆长轴和短轴
% # angle: 矢量，椭圆倾斜角度，单位；弧度，长轴与x轴夹角
%
% Reference
% # opencv fitEllipse


m = size(points, 1);
assert(m >= 6);
rp = NaN(5, 1);
center = NaN(1, 2);
semiAxes = NaN(1, 2);
min_eps = 1e-8;

c = mean(points);
p = points - c;
Ad1 = NaN(m, 5);
for iPoint = 1:1:m
    Ad1(iPoint, 1) = -p(iPoint, 1) * p(iPoint, 1);
    Ad1(iPoint, 2) = -p(iPoint, 2) * p(iPoint, 2);
    Ad1(iPoint, 3) = -p(iPoint, 1) * p(iPoint, 2);
    Ad1(iPoint, 4) = p(iPoint, 1);
    Ad1(iPoint, 5) = p(iPoint, 2);
end
bd1 = zeros(m, 1) + 10000.0;
x1 = Ad1 \ bd1;
gfp = x1;

Ad2 = [2*gfp(1, 1), gfp(3, 1);
    gfp(3, 1), 2*gfp(2, 1)];
bd2 = [gfp(4, 1); gfp(5, 1)];
x2 = Ad2 \ bd2;
rp(1:2, 1) = x2;

Ad3 = NaN(m, 3);
for iPoint = 1:1:m
    Ad3(iPoint, 1) = (p(iPoint, 1)-rp(1, 1)) * (p(iPoint, 1)-rp(1, 1));
    Ad3(iPoint, 2) = (p(iPoint, 2)-rp(2, 1)) * (p(iPoint, 2)-rp(2, 1));
    Ad3(iPoint, 3) = (p(iPoint, 1)-rp(1, 1)) * (p(iPoint, 2)-rp(2, 1));
end
bd3 = zeros(m, 1) + 1.0;
x3 = Ad3 \ bd3;
gfp(1:3, 1) = x3;

rp(5, 1) = -0.5 * atan2(gfp(3, 1), gfp(2, 1) - gfp(1, 1));
if abs(gfp(3, 1)) > min_eps
    t = gfp(3, 1) / sin(-2.0 * rp(5, 1));
else
    t = gfp(2, 1) - gfp(1, 1);
end
rp(3, 1) = abs(gfp(1, 1) + gfp(2, 1) - t);
if rp(3, 1) >min_eps
    rp(3, 1) = sqrt(2.0 / rp(3, 1));
end    
rp(4, 1) = abs(gfp(1, 1) + gfp(2, 1) + t);
if rp(4, 1) >min_eps
    rp(4, 1) = sqrt(2.0 / rp(4, 1));
end  

center(1, 1) = rp(1, 1) + c(1);
center(1, 2) = rp(2, 1) + c(2);
width = rp(3, 1)*2;
height = rp(4, 1)*2;

if(width < height)
    tmp = width;
    width = height;
    height = tmp;
    angle = 90 + rp(5, 1)*180/pi;
else
    angle = rp(5, 1)*180/pi;
end

if angle < -180
    angle = angle + 360;
end
if angle >360
    angle = angle - 360;
end
angle = angle*pi/180;
semiAxes(1, 1) = 0.5 * width;
semiAxes(1, 2) = 0.5 * height;
end

